#include "error_visualization.hpp"
using namespace std;

std::vector<Eigen::Vector3d> point_vector1;
std::vector<Eigen::Vector3d> point_vector2;
std::vector<Eigen::Vector3d> point_vector3;
std::vector<Eigen::Vector3d> point_vector4;
pcl::PointCloud<pcl::PointXYZI> cloud;
sensor_msgs::PointCloud2 output_cloud;

int main(int argc, char **argv)
{
    ros::init(argc, argv, "error_visualization");
    ros::NodeHandle nh;
    nh.param("gps_yuandian_lon",gps_yuandian_lon,116.23438706);
    nh.param("gps_yuandian_lat",gps_yuandian_lat,39.46142205);
    nh.param("asml_height",asml_height,14.3);
    

    human_point_pub = nh.advertise<sensor_msgs::PointCloud2>("/zhou/human_point", 100);
    car_point_pub = nh.advertise<sensor_msgs::PointCloud2>("/zhou/car_point", 100);
    oil_point_pub = nh.advertise<sensor_msgs::PointCloud2>("/zhou/oil_point", 100);
    weapon_point_pub = nh.advertise<sensor_msgs::PointCloud2>("/zhou/weapon_point", 100);

    pcl_pub = nh.advertise<sensor_msgs::PointCloud2> ("/zhou/pcl_output", 1);

    txt_pub = nh.advertise<std_msgs::Float32MultiArray> ("/zhou/score", 1);

    pcl::io::loadPCDFile ("/home/unionsys/swarm_ws/src/wlcb_dimianzhan/pose_file/map.pcd", cloud);
    pcl::toROSMsg(cloud, output_cloud);
    output_cloud.header.frame_id = "map";

    
    ifstream human_file_estimate;
	human_file_estimate.open("/home/unionsys/swarm_ws/src/wlcb_dimianzhan/pose_file/human_coord.txt"); 
    string linestr1;
    Eigen::Vector3d temp1;
    while (getline(human_file_estimate, linestr1))
	{
        std::stringstream ss1(linestr1);
        ss1 >> temp1(0);
        ss1 >> temp1(1);
        ss1 >> temp1(2);
        point_vector1.push_back(temp1);
    }
    human_file_estimate.close();

    ifstream car_file_estimate;
	car_file_estimate.open("/home/unionsys/swarm_ws/src/wlcb_dimianzhan/pose_file/car_coord.txt"); 
    string linestr2;
    Eigen::Vector3d temp2;
    while (getline(car_file_estimate, linestr2))
	{
        std::stringstream ss2(linestr2);
        ss2 >> temp2(0);
        ss2 >> temp2(1);
        ss2 >> temp2(2);
        point_vector2.push_back(temp2);
    }
    car_file_estimate.close();

    ifstream oil_file_estimate;
	oil_file_estimate.open("/home/unionsys/swarm_ws/src/wlcb_dimianzhan/pose_file/oil_coord.txt"); 
    string linestr3;
    Eigen::Vector3d temp3;
    while (getline(oil_file_estimate, linestr3))
	{
        std::stringstream ss3(linestr3);
        ss3 >> temp3(0);
        ss3 >> temp3(1);
        ss3 >> temp3(2);
        point_vector3.push_back(temp3);
    }
    oil_file_estimate.close();

    ifstream weapon_file_estimate;
	weapon_file_estimate.open("/home/unionsys/swarm_ws/src/wlcb_dimianzhan/pose_file/weapon_coord.txt"); 
    string linestr4;
    Eigen::Vector3d temp4;
    while (getline(weapon_file_estimate, linestr4))
	{
        std::stringstream ss4(linestr4);
        ss4 >> temp4(0);
        ss4 >> temp4(1);
        ss4 >> temp4(2);
        point_vector4.push_back(temp4);
    }
    weapon_file_estimate.close();

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_to_pub1(new pcl::PointCloud<pcl::PointXYZ>);
    for(int i=0;i<point_vector1.size();i++)
    {
        pcl::PointXYZ pt;
        
        pt.x = point_vector1[i](0);
        pt.y = point_vector1[i](1);
        pt.z = point_vector1[i](2);
        cloud_to_pub1->points.push_back(pt);

    }

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_to_pub2(new pcl::PointCloud<pcl::PointXYZ>);
    for(int i=0;i<point_vector2.size();i++)
    {
        pcl::PointXYZ pt;
        
        pt.x = point_vector2[i](0);
        pt.y = point_vector2[i](1);
        pt.z = point_vector2[i](2);
        cloud_to_pub2->points.push_back(pt);

    }

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_to_pub3(new pcl::PointCloud<pcl::PointXYZ>);
    for(int i=0;i<point_vector3.size();i++)
    {
        pcl::PointXYZ pt;
        
        pt.x = point_vector3[i](0);
        pt.y = point_vector3[i](1);
        pt.z = point_vector3[i](2);
        cloud_to_pub3->points.push_back(pt);

    }

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_to_pub4(new pcl::PointCloud<pcl::PointXYZ>);
    for(int i=0;i<point_vector4.size();i++)
    {
        pcl::PointXYZ pt;
        
        pt.x = point_vector4[i](0);
        pt.y = point_vector4[i](1);
        pt.z = point_vector4[i](2);
        cloud_to_pub4->points.push_back(pt);

    }

    ros::Rate rate(10);
    //main loop here
    while(ros::ok()){
        pcl_pub.publish(output_cloud);

        sensor_msgs::PointCloud2 t_rosCloud1;
        pcl::toROSMsg(*cloud_to_pub1,t_rosCloud1);
        t_rosCloud1.header.frame_id = "map";
        human_point_pub.publish(t_rosCloud1);

        sensor_msgs::PointCloud2 t_rosCloud2;
        pcl::toROSMsg(*cloud_to_pub2,t_rosCloud2);
        t_rosCloud2.header.frame_id = "map";
        car_point_pub.publish(t_rosCloud2);

        sensor_msgs::PointCloud2 t_rosCloud3;
        pcl::toROSMsg(*cloud_to_pub3,t_rosCloud3);
        t_rosCloud3.header.frame_id = "map";
        oil_point_pub.publish(t_rosCloud3);

        sensor_msgs::PointCloud2 t_rosCloud4;
        pcl::toROSMsg(*cloud_to_pub4,t_rosCloud4);
        t_rosCloud4.header.frame_id = "map";
        weapon_point_pub.publish(t_rosCloud4);


        ros::spinOnce();
        rate.sleep();
    }
    return 0;
}
